﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Woodstock.Common;
using Phidgets;
using System.ComponentModel;
using Phidgets.Events;

namespace Woodstock.HAL.Phidgets
{
    /// <summary>
    /// Defines a motor controlled by PhidgetAdvancedServo 8-Motor
    /// 
    /// http://www.phidgets.com/products.php?category=11&product_id=1061
    /// </summary>
    public class PhidgetServoMotor : Motor, IDisposable
    {
        public int ServoIndex { get; set; }
        public double PositionMax { get; set; }
        public double PositionMinimum { get; set; }

        /*private bool _engaged;
        public bool Engaged
        {
            get { return _engaged; }
            set { _engaged = value; NotifyPropertyChanged("Engaged"); }
        }*/

        private bool _attached;
        public bool Attached
        {
            get { return _attached; }
            set { _attached = value; NotifyPropertyChanged("Attached"); }
        }



        private static AdvancedServo servoController;

        public PhidgetServoMotor(int servoIndex, double positionMax, double positionMinimum)
        {
            if (servoController == null)
            {   
                servoController = new AdvancedServo();
            }
            
            servoController.Attach += new AttachEventHandler(servoController_Attach);
            servoController.Detach += new DetachEventHandler(servoController_Detach);
            servoController.open();            
        

            ServoIndex = servoIndex;
            PositionMax = positionMax;
            PositionMinimum = positionMinimum;
        }

        void SetupMotor()
        {
            lock (servoController)
            {
                servoController.servos[ServoIndex].Engaged = false;

                servoController.servos[ServoIndex].Type = ServoServo.ServoType.RAW_us_MODE;
                servoController.servos[ServoIndex].Acceleration = servoController.servos[ServoIndex].AccelerationMax;
                servoController.servos[ServoIndex].VelocityLimit = servoController.servos[ServoIndex].VelocityMax;

                servoController.servos[ServoIndex].PositionMax = PositionMax;
                servoController.servos[ServoIndex].PositionMin = PositionMinimum;
                servoController.servos[ServoIndex].Position = PositionMinimum;

                servoController.servos[ServoIndex].Engaged = true;
            }
        }


        void servoController_Detach(object sender, DetachEventArgs e)
        {
            Attached = false;
        }

        void servoController_Attach(object sender, AttachEventArgs e)
        {
            Attached = true;
            
            SetupMotor();            
        }

        
        public override double CurrentPosition
        {
            get
            {
                if (!servoController.Attached || servoController.servos.Count < ServoIndex)
                {
                    throw new InvalidOperationException();
                }

                return servoController.servos[ServoIndex].Current;
            }
        }

        private double _position;
        public override double Position
        {
            get
            {
                return _position;
            }
            set
            {
                double tempPosition = 0;

                if (value >= 100)
                    tempPosition = 100;
                else if (value <= 0)
                    tempPosition = 0;
                else
                    tempPosition = value;

                // Set the position internal percentage value
                _position = tempPosition;
                
                // Figure out what makes 1%
                double percent = (PositionMax - PositionMinimum) / 100;

                servoController.servos[ServoIndex].Position = PositionMinimum + (percent * _position);
                                
                NotifyPropertyChanged("Position");
            }
        }

        #region IDisposable Members

        public void Dispose()
        {
            servoController.servos[ServoIndex].Engaged = false;
        }

        #endregion
    }
}
